﻿using System;
using Common;

namespace TeamIce
{
	public enum Border
	{
		None,
		Left,
		Top,
		Right,
		Bottom
	}

	public static class IceMath
	{
		#region SimpleMath

		public static Vector BoundPoint(Vector p)
		{
			if (p.X < 0) p.X = 0;
			if (p.Y < 0) p.Y = 0;
			if (p.X > Field.Borders.Width) p.X = Field.Borders.Width;
			if (p.Y > Field.Borders.Height) p.Y = Field.Borders.Height;
			return p;
		}

		public static Vector BoundPointOnAngle(Vector fromPoint, double angle, int dx = 100)
		{
			Vector point = PointOnAngle(fromPoint, angle, dx);
			if (!Field.Borders.Contains(point))
			{
				int dist = Math.Min(Math.Abs(dx - 1), 100);

				while (!Field.Borders.Contains(point))
				{
					point = PointOnAngle(fromPoint, angle, dist);
					dist--;
					if (dist <= 0)
						return fromPoint;
				}
			}
			return point;
		}

		//Возвращает угол (0..PI) между двумя векторами с начальной точкой в 0,0
		public static double AngleBetween(Vector v1, Vector v2)
		{
			float l1 = v1.Length;
			float l2 = v2.Length;
			if (l1 < IceConst.EPS || l2 < IceConst.EPS)
				return Double.NaN;
			double d = (v1.X * v2.X + v1.Y * v2.Y) / (l1 * l2);
			double angle = Math.Acos(d);
			return Double.IsNaN(angle) ? 0 : angle;
		}

		//Возвращает -1, если v2 находится по часовой стрелке относительно v1
		//1 - если против часовой стрелки
		//0 - если направления векторов совпадают или противоположны
		public static int IsVector2ClockwiseFromVector1(Vector v1, Vector v2)
		{
			return Math.Sign(v1.X * v2.Y - v2.X * v1.Y);
		}

		/// <summary>
		/// Определение положения точки относительно прямой
		/// </summary>
		/// <param name="linePoint1">первая точка задающая линию</param>
		/// <param name="linePoint2">вторая точка задающая линию</param>
		/// <param name="testPoint">
		/// точка, положение которой необходимо узнать
		/// </param>
		/// <returns>
		///  1 - точка слева от прямой
		///  0 - точка принадлежит прямой
		/// -1 - точка справа от прямой
		/// </returns>
		static public int PointNearLine(Vector linePoint1,
										Vector linePoint2,
										Vector testPoint)
		{
			float tmp = (linePoint2.X - linePoint1.X)*(testPoint.Y - linePoint1.Y) -
			            (linePoint2.Y - linePoint1.Y)*(testPoint.X - linePoint1.X);

			if (tmp > 0)
				return 1;
			if (tmp < 0)
				return -1;
			return 0;
		}

		//Возвращает 1, если угол нужно увеличивать
		//-1 - если уменьшать
		public static int GetVelToSideSign(Vector relPos, Vector vel)
		{
			Vector point = relPos + vel;
			if (PointNearLine(Vector.Zero, relPos, point) > 0)
				return -1;
			return 1;
		}

		public static bool IsVelToSideDirectedToBall(Vector relPos, Vector vel)
		{
			//Увеличиваем длину относительной позиции, чтобы длина скорости (до 3)
			//не влияла на сравнение
			Vector farRelPos = relPos*1000;
			Vector point = farRelPos + vel;
			return farRelPos.LengthSquared > point.LengthSquared;
		}

		//Возвращает четверть в которой лежит точка, относительно точки (0, 0)
		public static int GetQuarter(Vector relPos)
		{
			if (relPos.X >= 0)
				return relPos.Y > 0 ? 4 : 1;
			return relPos.Y > 0 ? 3 : 2;
		}

		//Приводит угол к диапазону от 0 до 2Пи
		public static double AngleNormalize(double alpha)
		{
			while (alpha < 0) alpha += 2 * Math.PI;
			while (alpha > 2 * Math.PI) alpha -= 2 * Math.PI;
			return alpha;
		}

		//Возвращает четверть в которой лежит угол (в радианах)
		public static int GetQuarter(double alpha)
		{
			while (alpha < 0) alpha += Math.PI * 2;
			while (alpha >= Math.PI * 2) alpha -= Math.PI * 2;
			if (alpha >= 3 * Math.PI / 2) return 4;
			if (alpha >= Math.PI) return 3;
			return alpha >= Math.PI / 2 ? 2 : 1;
		}

		//Возвращает точку на перпендикуляре к прямой (Zero, relPos)
		public static Vector GetPerpend(Vector relPos)
		{
			return Math.Abs(relPos.Y) < IceConst.EPS
					? new Vector(-relPos.Y / relPos.X, 1)
					: new Vector(1, -relPos.X / relPos.Y);
		}

		private static bool IntersectInternal(double from1, double to1, double from2, double to2)
		{
			return
				((from2 >= from1) && (from2 <= to1)) ||
				((to2 >= from1) && (to2 <= to1)) ||
				((from2 <= from1) && (to2 >= to1));
		}

		//Пересекаются ли два сектора
		public static bool Intersect(double from1, double to1, double from2, double to2)
		{
			if (Double.IsNaN(from1) || Double.IsNaN(to1) ||
				Double.IsNaN(from2) || Double.IsNaN(to2))
				return false;

			double tmpAlphaFrom = from2;
			double tmpAlphaTo = to2;

			if (IntersectInternal(from1, to1, tmpAlphaFrom, tmpAlphaTo))
				return true;

			tmpAlphaFrom = from2 + 2 * Math.PI;
			tmpAlphaTo = to2 + 2 * Math.PI;

			if (IntersectInternal(from1, to1, tmpAlphaFrom, tmpAlphaTo))
				return true;

			tmpAlphaFrom = from2 - 2 * Math.PI;
			tmpAlphaTo = to2 - 2 * Math.PI;

			return IntersectInternal(from1, to1, tmpAlphaFrom, tmpAlphaTo);
		}

		//Пересекаются ли два сектора
		public static bool Intersect(Sector sec1, Sector sec2)
		{
			return Intersect(sec1.AlphaFrom, sec1.AlphaTo, sec2.AlphaFrom, sec2.AlphaTo);
		}

		//Возвращает точку (справа или слева от данной), на прямой, направленной под углом alpha
		public static Vector PointOnAngle(Vector p1, double alpha, int dx = 100)
		{
			dx = Math.Abs(dx);
			if (dx == 0)
				dx = 100;

			alpha = AngleNormalize(alpha);

			if (Math.Abs(alpha) < IceConst.EPS)
				return new Vector(p1.X + dx, p1.Y);
			if (Math.Abs(alpha - Math.PI / 2) < IceConst.EPS)
				return new Vector(p1.X, p1.Y - dx);
			if (Math.Abs(alpha - Math.PI) < IceConst.EPS)
				return new Vector(p1.X - dx, p1.Y);
			if (Math.Abs(alpha - (3 * Math.PI / 2)) < IceConst.EPS)
				return new Vector(p1.X, p1.Y + dx);

			bool onLeft = alpha > Math.PI / 2 && alpha < 3 * Math.PI / 2;
			double x = p1.X + (onLeft ? -dx : dx);

			double dy;
			double y = p1.Y;

			int quarter = GetQuarter(alpha);
			switch (quarter)
			{
				case 1:
					if (alpha < Math.PI / 2)
						dy = dx * Math.Tan(alpha);
					else
					{
						alpha = Math.PI / 2 - alpha;
						dy = dx / Math.Tan(alpha);
					}
					y = p1.Y - dy;
					break;
				case 2:
					if (alpha < 3 * Math.PI / 4)
					{
						alpha = Math.PI / 4 - (3 * Math.PI / 4 - alpha);
						dy = dx / Math.Tan(alpha);
					}
					else
					{
						alpha = Math.PI - alpha;
						dy = dx * Math.Tan(alpha);
					}
					y = p1.Y - dy;
					break;
				case 3:
					if (alpha < 5 * Math.PI / 4)
					{
						alpha = alpha - Math.PI;
						dy = dx * Math.Tan(alpha);
					}
					else
					{
						alpha = 3 * Math.PI / 2 - alpha;
						dy = dx / Math.Tan(alpha);
					}
					y = p1.Y + dy;
					break;
				case 4:
					if (alpha < 7 * Math.PI / 4)
					{
						alpha = Math.PI / 4 - (7 * Math.PI / 4 - alpha);
						dy = dx / Math.Tan(alpha);
					}
					else
					{
						alpha = 2 * Math.PI - alpha;
						dy = dx * Math.Tan(alpha);
					}
					y = p1.Y + dy;
					break;
			}
			return new Vector(x, y);
		}

		//Возвращает точку за полем, отражение по линии border
		public static Vector ReflectPoint(Vector point, Border border)
		{
			switch (border)
			{
				case Border.Left:
					return new Vector(-point.Position.X, point.Position.Y);
				case Border.Top:
					return new Vector(point.Position.X, -point.Position.Y);
				case Border.Right:
					return new Vector(2 * Field.Borders.Width - point.Position.X, point.Position.Y);
				case Border.Bottom:
					return new Vector(point.Position.X, 2 * Field.Borders.Height - point.Position.Y);
				default:
					return new Vector(point.X, point.Y);
			}
		}

		public static IcePlayer ReflectPlayer(IcePlayer player, Border border)
		{
			return new IcePlayer(player)
			    {
			        Position = ReflectPoint(player.Position, border),
			        Velocity = ReflectPoint(player.Velocity, border)
			    };
		}
		#endregion

		#region BallMath
		//Формула для получения позиции мяча в любой момент времени t
		//Pt = P0 + V0 * (((a^t)-1)/(a-1))
		//где P0 - начальная позиция, V0 - начальная скорость,
		//a - коэф. торможения мяча BallSlowDownFactor
		public static Vector CalcBallPos(Vector posStart, Vector velStart, int steps, bool withReflection = true)
		{
			float a = Constants.BallSlowDownFactor;
			Vector pos = posStart + velStart * ((Math.Pow(a, steps) - 1) / (a - 1));
			if (withReflection)
			{
				if (pos.X < 0) pos.X = -pos.X;
				if (pos.Y < 0) pos.Y = -pos.Y;
				if (pos.X > Field.Borders.Width) pos.X = pos.X - Field.Borders.Width;
				if (pos.Y > Field.Borders.Height) pos.Y = pos.Y - Field.Borders.Height;
			}
			return pos;
		}

        //Возвращает время, необходимое мячу, чтобы пройти расстояние dist
        //t = log(a*dist/v-dist/v+1)/log(a)
        //где v - начальная скорость мяча
        //a - коэф. торможения мяча BallSlowDownFactor
        public static int GetBallTimeForDistance(float dist, Vector velStart)
        {
            float a = Constants.BallSlowDownFactor;
            float v = velStart.Length;
            return (int) (Math.Log(a*dist/v - dist/v + 1)/Math.Log(a)) + 1;
        }

		//Возвращает точку, в которой мяч отразится от стенки в первый раз
		public static Vector GetFirstBallReflectPoint(Vector ballPos, Vector ballVel, out Vector reflectVel)
		{
			//Мяч может отразиться только от двух стенок - по направлению его скоростей по x и y
			Line ballLine = new Line(ballPos, (ballPos + ballVel).Position);

			float x = ballVel.X < 0 ? 0 : Field.Borders.Width;
			Line vertLine = new Line(new Vector(x, 0), new Vector(x, 1));

			float y = ballVel.Y < 0 ? 0 : Field.Borders.Height;
			Line horizLine = new Line(new Vector(0, y), new Vector(1, y));

			Vector res = Vector.Zero;
			if (Line.Intersect(ballLine, vertLine, ref res))
				if ((res.Y >= 0) && (res.Y <= Field.Borders.Height))
				{
					reflectVel.X = -ballVel.X;
					reflectVel.Y = ballVel.Y;
					return res;
				}
			if (Line.Intersect(ballLine, horizLine, ref res))
				if ((res.X >= 0) && (res.X <= Field.Borders.Width))
				{
					reflectVel.X = ballVel.X;
					reflectVel.Y = -ballVel.Y;
					return res;
				}

			reflectVel = Vector.Zero;
			return Vector.Zero;
		}

		//Ищет от какой стенки рикошетиться, чтобы путь от источника к цели был наименьшим
		//Возвращает точку рикошета и линию от которой он произойдет
		internal static Border ClosestReflectPoint(Vector p1, Vector p2, out Vector reflectPoint)
		{
			float horizDist = Math.Min(p1.Y + p2.Y, (Field.Borders.Height - p1.Y) + (Field.Borders.Height - p2.Y));
			Vector horizReflectPoint = Vector.Zero;
			Border horizBorder;
			if (p1.Y + p2.Y < (Field.Borders.Height - p1.Y) + (Field.Borders.Height - p2.Y))
			{
				Line.Intersect(
					new Line(p1, new Vector(p2.X, -p2.Y)),
					new Line(new Vector(0, 0), new Vector(Field.Borders.Width, 0)), ref horizReflectPoint);
				horizBorder = Border.Top;
			}
			else
			{
				Line.Intersect(
					new Line(p1, new Vector(p2.X, 2 * Field.Borders.Height - p2.Y)),
					new Line(new Vector(0, Field.Borders.Height), new Vector(Field.Borders.Width, Field.Borders.Height)),
					ref horizReflectPoint);
				horizBorder = Border.Bottom;
			}

			float vertDist = Math.Min(p1.X + p2.X, (Field.Borders.Width - p1.X) + (Field.Borders.Width - p2.X));
			Vector vertReflectPoint = Vector.Zero;
			Border vertBorder;
			if (p1.X + p2.X < (Field.Borders.Width - p1.X) + (Field.Borders.Width - p2.X))
			{
				Line.Intersect(
					new Line(p1, new Vector(-p2.X, p2.Y)),
					new Line(new Vector(0, 0), new Vector(0, Field.Borders.Height)), ref vertReflectPoint);
				vertBorder = Border.Left;
			}
			else
			{
				Line.Intersect(
					new Line(p1, new Vector(2 * Field.Borders.Width - p2.X, p2.Y)),
					new Line(new Vector(Field.Borders.Width, 0), new Vector(Field.Borders.Width, Field.Borders.Height)),
					ref vertReflectPoint);
				vertBorder = Border.Right;
			}
			reflectPoint =  horizDist < vertDist ? horizReflectPoint : vertReflectPoint;
			return horizDist < vertDist ? horizBorder : vertBorder;
		}
		#endregion

		#region PlayerMath
		//По радиусу игрока получает углы, в которых он может поймать мяч
		public static void RadiusToAngles(int radius, Vector relPos, out double alphaFrom, out double alphaTo)
		{
			Vector perpend = GetPerpend(relPos);

			Vector posMin = relPos - perpend.Unit() * radius;
			Vector posMax = relPos + perpend.Unit() * radius;

			alphaFrom = AngleBetween(posMin, new Vector(1, 0));
			alphaTo = AngleBetween(posMax, new Vector(1, 0));

			if (Double.IsNaN(alphaFrom) || Double.IsNaN(alphaTo))
				return;

			int q1 = GetQuarter(posMin);
			int q2 = GetQuarter(posMax);

			//Если точка находится в 3(-+) или 4(++) четверти, то угол нужно сделать больше Пи
			if (q1 > 2) alphaFrom = Math.PI + Math.PI - alphaFrom;
			if (q2 > 2) alphaTo = Math.PI + Math.PI - alphaTo;

			//определяем какой угол меньше Pi: alphaFrom..alphaTo или alphaTo..alphaFrom
			//Сначала гарантируем, что alphaFrom < alphaTo
			if (alphaTo < alphaFrom)
			{
				double tmp = alphaFrom;
				alphaFrom = alphaTo;
				alphaTo = tmp;
			}

			if (alphaTo - alphaFrom > Math.PI)
			{
				double tmp = alphaFrom;
				alphaFrom = alphaTo - 2 * Math.PI;
				alphaTo = tmp;
			}
		}

		//Получает позицию игрока на момент времени t, 
		//по начальной позиции и скорости,
		//если он будет идти к цели на каждом ходу делая ActionGo(goal)
		//формула дает небольшую погрешность, если начальная скорость игрока не направлена в сторону цели
		public static Vector CalcPlayerMovementPos(Vector posStart, Vector velStart, Vector goal, int t)
		{
			float a = Constants.PlayerSlowDownFactor;
			double b = (Math.Pow(a, t + 1) - 1) / (a - 1) - 1;
			Vector c = (goal - posStart).Unit() * Constants.PlayerMaxVelocity * Constants.PlayerAccelerationFactor * a;
			Vector pos = posStart + velStart * b + c * (b - t) / (a - 1);
			return pos;
		}

		//Возвращает время за которое игрок догонит мяч, если оба будут двигаться равномерно
		//с заданными скоростями
		//0 - если игрок не догонит мяч
		//dt - на сколько больше ходов может сделать игрок чем мяч
		public static int CalcTimeToCatchWithConstVelocities(Vector ballPos, Vector ballVel, Vector playerPos,
			                                              float playerVel, int dt = 0)
		{
			ballPos -= dt * ballVel;

			float u = playerVel;
			float vx = ballVel.X;
			float vy = ballVel.Y;
			float px = playerPos.X;
			float py = playerPos.Y;
			float bx = ballPos.X;
			float by = ballPos.Y;

			double a = vx * vx + vy * vy - u * u;
			double b = 2 * bx * vx - 2 * px * vx /*- 2*dt*vx*vx*/+
			           2 * @by * vy - 2 * py * vy /*- 2*dt*vy*vy*/;
			double c = py * py - 2 * @by * py + @by * @by /*+ /*dt*dt*vx*vx + 2*dt*px*vx - 2*b*dt*vx*/+
			           px * px - 2 * bx * px + bx * bx /*+ dt*dt*vy*vy + 2*dt*py*vy - 2*b*dt*vy*/;

			int t;
			if (Math.Abs(a) < IceConst.EPS)
			{
				t = (int)Math.Ceiling(-c / b);
			}
			else
			{
				double d = b * b - 4 * a * c;
				if (d < 0) t = 0;
				else
				{
					double t1 = (-b + Math.Sqrt(d)) / (2 * a);
					double t2 = (-b - Math.Sqrt(d)) / (2 * a);

					if (t1 < 0 && t2 < 0)
						return 0;
					if (t1 < 0) t = (int)Math.Ceiling(t2);
					else if (t2 < 0) t = (int)Math.Ceiling(t1);
					else t = (int)Math.Ceiling(Math.Min(t1, t2));
				}
			}
			if (t < dt) return 0;
			return t - dt;
		}

        /*
		//Возвращает точку, в которую нужно идти игроку, чтобы перехватить мяч
		//при условии что оба будут двигаться равномерно.
		//Учитываются отражения мяча от стенок
		public static VectorSteps CatchBallWithConstVelocities(Vector ballPos, Vector ballVel, Vector playerPos,
			                                                float playerVel, bool decreaseVelOnReflection = true,
			                                                int stepsBefore = 0)
		{
			if (playerVel < IceConst.EPS)
				return new VectorSteps(Vector.Zero, 0);

			if (ballVel.LengthSquared < IceConst.EPS)
				return new VectorSteps(ballPos, (int)Math.Ceiling(Vector.Distance(playerPos, ballPos) / playerVel));

            Vector reflectVel;
            Vector reflectPoint = GetFirstBallReflectPoint(ballPos, ballVel, out reflectVel);

            //Грубое кол-во ходов
            int averageSteps = (int)Math.Ceiling(Vector.Distance(ballPos, reflectPoint) / ballVel.Length);
		    
            float reflectDist = Vector.Distance(ballPos, reflectPoint);
            //Точное кол-во ходов, за которое мяч долетит до стены
            int reflectSteps = GetBallTimeForDistance(reflectDist, ballVel);
            if (decreaseVelOnReflection)
                reflectVel *= Math.Pow(Constants.BallSlowDownFactor, reflectSteps);

            //Находим количество ходов, в случае, если мяч не будет замедляться
            Vector averageBallVel = new Vector(ballVel.X, ballVel.Y);
            int steps = CalcTimeToCatchWithConstVelocities(ballPos, averageBallVel, playerPos, playerVel, stepsBefore);
		    while (true)
		    {
                float dist = steps * averageBallVel;
		    }
            
            //Если догонит даже в этом случае, то постепенно
            if (steps > 0 && steps <= reflectSteps)
            {
                return new VectorSteps(ballPos + steps*ballVel, steps);
            }

		    //int steps = CalcTimeToCatchWithConstVelocities(ballPos, ballVel.Unit() * reflectDist / reflectSteps, playerPos, playerVel, stepsBefore);

			

			//Игрок догонит мяч уже за линией поля или вообще не догонит по данной траектории
			//Поэтому считаем, что мяч движется по отраженной траектории
			VectorSteps res = CatchBallWithConstVelocities(reflectPoint, reflectVel, playerPos, playerVel,
			                                               decreaseVelOnReflection, stepsBefore + reflectSteps);
			res.Steps += reflectSteps;
			return res;
		}*/

        //Возвращает точку, в которую нужно идти игроку, чтобы перехватить мяч
        //при условии что оба будут двигаться равномерно.
        //Учитываются отражения мяча от стенок
        public static VectorSteps CatchBallWithConstVelocities(Vector ballPos, Vector ballVel, Vector playerPos,
                                                                float playerVel, bool decreaseVelOnReflection = true,
                                                                int stepsBefore = 0)
        {
            if (playerVel < IceConst.EPS)
                return new VectorSteps(Vector.Zero, 0);

            if (ballVel.LengthSquared < IceConst.EPS)
                return new VectorSteps(ballPos, (int)Math.Ceiling(Vector.Distance(playerPos, ballPos) / playerVel));

            int steps = CalcTimeToCatchWithConstVelocities(ballPos, ballVel, playerPos, playerVel, stepsBefore);

            Vector reflectVel;
            Vector reflectPoint = GetFirstBallReflectPoint(ballPos, ballVel, out reflectVel);
            int reflectSteps = (int)Math.Ceiling(Vector.Distance(ballPos, reflectPoint) / ballVel.Length);
            //float reflectDist = Vector.Distance(ballPos, reflectPoint);
            //Точное кол-во ходов, за которое мяч долетит до стены
            //int realReflectSteps = GetBallTimeForDistance(reflectDist, ballVel);
            if (decreaseVelOnReflection)
                reflectVel *= Math.Pow(Constants.BallSlowDownFactor, reflectSteps);

            if (steps > 0 && steps <= reflectSteps)
                return new VectorSteps(ballPos + steps * ballVel, steps);

            //Игрок догонит мяч уже за линией поля или вообще не догонит по данной траектории
            //Поэтому считаем, что мяч движется по отраженной траектории
            VectorSteps res = CatchBallWithConstVelocities(reflectPoint, reflectVel, playerPos, playerVel,
                                                           decreaseVelOnReflection, stepsBefore + reflectSteps);
            res.Steps += reflectSteps;
            return res;
        }
		#endregion
	}
}
